
#include "AttitudeLimit.h"
#include "rtapi/rtapi.h" /* rtapi_print_msg */
#include <math.h>
#include "kinematics/kinematics.h"
#include "motion/mot_priv.h"
#include "motion/motion.h"
#include "motion/motion_debug.h"
#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/robot_mgr.h"
#include "kine/kine.h"

//add by ybh
int AttitudeRestricted(const ZucPose* toolOffset,
                       const double* joint,
                       ZucPose* world,
                       const KINEMATICS_FORWARD_FLAGS* fflags,
                       KINEMATICS_INVERSE_FLAGS* iflags,
                       const PmRpy* base_offset,
                       const ZucPose* user_offset,
                       const double tilt,
                       const double pan)
{
    //rtapi_print("angle = %f %f %f %f %f %f \n",joint[0],joint[1],joint[2],joint[3],joint[4],joint[5]);
    double x_tilt = tilt * PM_PI / 180.0;
    double z_pan = pan * PM_PI / 180.0;
    double s_x_tilt = sin(x_tilt);
    double c_x_tilt = cos(x_tilt);
    double s_z_pan = sin(z_pan);
    double c_z_pan = cos(z_pan);
    double d[6] = {toolOffset->tran.x, toolOffset->tran.y, toolOffset->tran.z, toolOffset->a, toolOffset->b, toolOffset->c};
    PmRotationMatrix pos_off, pos_off_x, pos_off_final, R_x, R_z;
    PmRpy rpy_off, rpy_final;
    rpy_off.y = d[3] * PM_PI / 180.0;
    rpy_off.p = d[4] * PM_PI / 180.0;
    rpy_off.r = d[5] * PM_PI / 180.0;
    pmRpyMatConvert(&rpy_off, &pos_off);
    R_x.x.x = 1;
    R_x.y.x = 0;
    R_x.z.x = 0;
    R_x.x.y = 0;
    R_x.y.y = c_x_tilt;
    R_x.z.y = -s_x_tilt;
    R_x.x.z = 0;
    R_x.y.z = s_x_tilt;
    R_x.z.z = c_x_tilt;

    R_z.x.x = c_z_pan;
    R_z.y.x = -s_z_pan;
    R_z.z.x = 0;
    R_z.x.y = s_z_pan;
    R_z.y.y = c_z_pan;
    R_z.z.y = 0;
    R_z.x.z = 0;
    R_z.y.z = 0;
    R_z.z.z = 1;
    // rtapi_print(" ------------------------------------\n");

    // rtapi_print(" %f %f %f\n",R_z.x.x,R_z.y.x,R_z.z.x);
    // rtapi_print(" %f %f %f\n",R_z.x.y,R_z.y.y,R_z.z.y);
    // rtapi_print(" %f %f %f\n",R_z.x.z,R_z.y.z,R_z.z.z);
    // rtapi_print(" ------------------------------------\n");
    pmMatMatMult(&pos_off, &R_z, &pos_off_x);
    pmMatMatMult(&pos_off_x, &R_x, &pos_off_final);
    pmMatRpyConvert(&pos_off_final, &rpy_final);

    ZucPose Tool_Att;
    Tool_Att.tran.x = toolOffset->tran.x;
    Tool_Att.tran.y = toolOffset->tran.y;
    Tool_Att.tran.z = toolOffset->tran.z;
    Tool_Att.a = rpy_final.y * 180.0 / PM_PI;
    Tool_Att.b = rpy_final.p * 180.0 / PM_PI;
    Tool_Att.c = rpy_final.r * 180.0 / PM_PI;

    ZucPose wp;
    int ret = axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->kine()->fkine(RobJointVal(joint), wp, &Tool_Att, user_offset, base_offset);
    *world = wp;

    return ret;
}

int UpdateAttitudeLimit(double const* angle,
                        double conical_cenlineRpy[3],
                        const double deviation,
                        const double deviation_warning,
                        int* const flag_att,
                        const int tip_choice,
                        const double tilt,
                        const double pan)
{
    static double incl_angle;
    static double last_incl_angle;
    double delt_incl_angle;
    KINEMATICS_FORWARD_FLAGS fflag = 0;
    KINEMATICS_INVERSE_FLAGS iflag = 0;
    ZucPose restricted_att;

    //将偏斜、平移后的当做新的坐标系姿态
    //变换公式为: T{end to world} * T{ori_tool to end} * T{restri_tool to ori_tool},其中T{restri_tool to ori_tool}平移向量为{0}
    //圆锥几何示教在世界坐标系下，不需要输入用户坐标系进行计算
    ZucPose world_as_useroffset = {0};
    if (tip_choice == 0)  //TCP attitude are restricted
    {
        ZucPose tool_zeros;
        tool_zeros.tran.x = 0;
        tool_zeros.tran.y = 0;
        tool_zeros.tran.z = 0;
        tool_zeros.a = 0;
        tool_zeros.b = 0;
        tool_zeros.c = 0;
        tool_zeros.u = 0;
        tool_zeros.v = 0;
        tool_zeros.w = 0;
        AttitudeRestricted(&tool_zeros, angle, &restricted_att, &fflag, &iflag, &zucmotConfig->base_offset, &world_as_useroffset, tilt, pan);
    }
    else
    {
        AttitudeRestricted(&zucmotConfig->tool_offset, angle, &restricted_att, &fflag, &iflag, &zucmotConfig->base_offset, &world_as_useroffset, tilt, pan);
    }
    //rtapi_print("conical_cenlineRpy=%f %f %f---------- \n", conical_cenlineRpy[0],conical_cenlineRpy[1] ,conical_cenlineRpy[2]);
    //rtapi_print("tip_choice=%d--------- \n",tip_choice);
    //rtapi_print("tilt=%f--------- \n",tilt);
    //rtapi_print("pan=%f--------- \n",pan);
    //rtapi_print("deviation=%f---------- \n",deviation);
    double devia = deviation * PM_PI / 180.0;
    double devia_wran = (deviation - deviation_warning) * PM_PI / 180.0;
    PmRpy rpy_world, rpy_conical;
    PmCartesian world_z, conical_z;
    rpy_world.y = restricted_att.a * PM_PI / 180.0;
    rpy_world.p = restricted_att.b * PM_PI / 180.0;
    rpy_world.r = restricted_att.c * PM_PI / 180.0;
    rpy_conical.y = conical_cenlineRpy[0] * PM_PI / 180.0;
    rpy_conical.p = conical_cenlineRpy[1] * PM_PI / 180.0;
    rpy_conical.r = conical_cenlineRpy[2] * PM_PI / 180.0;
    ////rtapi_print("rpy=%f %f %f---------- \n",rpy_world.y, rpy_world.p ,rpy_world.r);
    PmRotationMatrix mat_world, mat_conical, R_x, R_z, mat_world_x, mat_world_final;
    pmRpyMatConvert(&rpy_world, &mat_world);
    pmRpyMatConvert(&rpy_conical, &mat_conical);
    world_z = mat_world.z;
    conical_z = mat_conical.z;
    double dot_vec = (world_z.x * conical_z.x + world_z.y * conical_z.y + world_z.z * conical_z.z) /
                     (pmSqrt(pmSq(world_z.x) + pmSq(world_z.y) + pmSq(world_z.z)) * pmSqrt(pmSq(conical_z.x) + pmSq(conical_z.y) + pmSq(conical_z.z)));
    if (dot_vec > 1.0)
        dot_vec = 1.0;
    else if (dot_vec < -1.0)
        dot_vec = -1.0;
    incl_angle = fabs(acos(dot_vec));
    if (zucmotStatus->attitude_limit_take_effect_flag == 1)
    {
        // rtapi_print("zucmotStatus->attitude_limit_take_effect_flag is 1\n");
        last_incl_angle = incl_angle;
    }
    delt_incl_angle = incl_angle - last_incl_angle;
    // rtapi_print("incl_angle=%f---------- \n", incl_angle);
    // rtapi_print("last_incl_angle=%f---------- \n", last_incl_angle);
    // rtapi_print("delt_incl_angle=%f---------- \n", delt_incl_angle);
    last_incl_angle = incl_angle;
    //rtapi_print("world_z=%f %f %f--------- \n",world_z.x,world_z.y,world_z.z);
    double delt_angle = 0.0;
    if (zucmotDebug->dragging)
    {
        delt_angle = 0.0001;
    }
    if (incl_angle >= devia && delt_incl_angle > delt_angle + CART_FUZZ)
    {
        *flag_att = 1;
    }
    else if (incl_angle < devia && incl_angle >= devia_wran && delt_incl_angle > delt_angle + CART_FUZZ && zucmotStatus->attitude_warning == 0)
    {
        zucmotStatus->attitude_warning = 1;
        *flag_att = 2;
    }
    else
    {
        *flag_att = 0;
    }
    if (zucmotStatus->attitude_warning == 1 && delt_incl_angle < -delt_angle - CART_FUZZ)
    {
        zucmotStatus->attitude_warning = 0;
    }
    return 0;
}
